#ifndef __DATA_CLASSIFIER_HPP
#define __DATA_CLASSIFIER_HPP

#include "ros/ros.h"
#include "vector"
#include <balloon_rl/nx_from_mcu.h>
#include <balloon_rl/nx_to_mcu.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
// #include <quadrotor_msgs/PositionCommand.h>

#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigen>


namespace DataClassifier
{

#define IMU_FEEDBACK_LEN    32
#define POS_INFO_LEN        40
#define POS_CONTROL_LEN     32
#define ACCELE_CONTROL_LEN  16
#define RL_OBS_LEN          24
#define RL_ACTION_LEN       12
#define MOTOR_SPD_LEN       24


typedef enum
{
    NO_ID              =    0x00,
    IMU_FEED_MSG       =    0xA1,
    POS_INFO_MSG       =    0xA2,
    POS_CONTROL_MSG    =    0xA3,
    ACCELE_CONTROL_MSG =    0xA4,
    RL_OBS_MSG         =    0xA5,
    RL_ACTION_MSG      =    0xA6,
    MOTOR_SPD_MSG      =    0xA7 
}msgID_e;

float U64ToFloat(uint64_t dat) 
{
    uint8_t buf[8];
    
    buf[0] = dat >> 56;
    buf[1] = dat >> 48;
    buf[2] = dat >> 40;
    buf[3] = dat >> 32;
    buf[4] = dat >> 24;
    buf[5] = dat >> 16;
    buf[6] = dat >> 8;
    buf[7] = dat & 0xFF;
    
    return *((float*)buf);
}

float U32ToFloat(uint32_t dat) 
{
    uint8_t buf[4];
    
    buf[0] = dat >> 24;
    buf[1] = dat >> 16;
    buf[2] = dat >> 8;
    buf[3] = dat & 0xFF;
    
    return *((float*)buf);
}

uint32_t FloatToU32(float dat) {
    uint8_t buf[4];
    
    buf[0] = ((uint8_t*)&dat)[0];
    buf[1] = ((uint8_t*)&dat)[1];
    buf[2] = ((uint8_t*)&dat)[2];
    buf[3] = ((uint8_t*)&dat)[3];
    
    return (buf[0] << 24) + (buf[1] << 16) + (buf[2] << 8) + buf[3];
}

inline void pack_int32(int32_t x, uint8_t *msgs, uint8_t &offset)
{
    msgs[offset++] = (uint8_t)(0xff & x);
    msgs[offset++] = (uint8_t)(0xff & x >> 8);
    msgs[offset++] = (uint8_t)(0xff & x >> 16);
    msgs[offset++] = (uint8_t)(0xff & x >> 24);
}

inline void pack_uint64(uint64_t x, uint8_t *msgs, uint8_t &offset)
{
    msgs[offset++] = (uint8_t)(0xff & x);
    msgs[offset++] = (uint8_t)(0xff & x >> 8);
    msgs[offset++] = (uint8_t)(0xff & x >> 16);
    msgs[offset++] = (uint8_t)(0xff & x >> 24);
    msgs[offset++] = (uint8_t)(0xff & x >> 32);
    msgs[offset++] = (uint8_t)(0xff & x >> 40);
    msgs[offset++] = (uint8_t)(0xff & x >> 48);
    msgs[offset++] = (uint8_t)(0xff & x >> 56);
}
inline void unpack_int32(int32_t &out, std::vector<uint8_t> &buff ,uint8_t &offset)
{
    out = buff[offset]         | buff[offset + 1]<<8  | \
          buff[offset + 2]<<16 | buff[offset + 3]<<24 ;

    offset =  offset + 4;
}

inline void unpack_uint64(uint64_t &out, std::vector<uint8_t> &buff ,uint8_t &offset)
{
    out = buff[offset]         | buff[offset + 1]<<8  | \
          buff[offset + 2]<<16 | buff[offset + 3]<<24 | \
          buff[offset + 4]<<32 | buff[offset + 5]<<40 | \
          buff[offset + 6]<<48 | buff[offset + 7]<<56 ;

    offset =  offset + 8;
}

msgID_e msgID_confirm(uint8_t ID, uint8_t data_len)
{ 
    if( ID == IMU_FEED_MSG && data_len == IMU_FEEDBACK_LEN )
    {  // nx使用
        return IMU_FEED_MSG;
    }

    if( ID == MOTOR_SPD_MSG && data_len == MOTOR_SPD_LEN )
    {  // nx使用
        return MOTOR_SPD_MSG;
    }

    if( ID == RL_OBS_MSG && data_len == RL_OBS_LEN )
    {  // nx使用
        return RL_OBS_MSG;
    }
    
    return NO_ID;
}

void McuMsg_Unpack(balloon_rl::nx_from_mcu *pNX_MCU, std::vector<uint8_t> &buff)
{
    uint8_t offset = 0;
    uint64_t timeStamp;
    int32_t height, roll, pitch, accZ;

    unpack_uint64(timeStamp, buff, offset);
    unpack_int32(height, buff, offset);
    unpack_int32(roll, buff, offset);
    unpack_int32(pitch, buff, offset);
    unpack_int32(accZ, buff, offset);
    

    pNX_MCU->header.stamp = ros::Time::now(); 
    pNX_MCU->timeStamp = U64ToFloat(timeStamp);
    pNX_MCU->height = U32ToFloat(height);
    pNX_MCU->roll = U32ToFloat(roll);
    pNX_MCU->pitch = U32ToFloat(pitch);
    pNX_MCU->accZ = U32ToFloat(accZ);
}

/******数据打包部分*******/

void McuData_pack(balloon_rl::nx_to_mcu *pNX_MCU, std::vector<uint8_t> &buff)
{
    buff.clear();
    buff.resize(RL_ACTION_LEN);

    uint8_t offset = 0; //head+ID+LEN

    pack_uint64( pNX_MCU->timeStamp, &buff[0], offset);
    pack_int32( pNX_MCU->thrust, &buff[0], offset);
}

}
#endif 
